import numpy as np
import pyproj
import matplotlib
import matplotlib.pyplot as plt
import cv2
import cartopy.crs as ccrs
import geopandas as gpd

from matplotlib.animation import FuncAnimation, FFMpegWriter
from matplotlib.patches import Rectangle, Polygon
from matplotlib.ticker import FormatStrFormatter
from cartopy.mpl.gridliner import LONGITUDE_FORMATTER, LATITUDE_FORMATTER
from shapely.geometry import Polygon, MultiLineString, LineString, Point
from shapely.ops import unary_union
from scipy.spatial import Voronoi
from rdp import rdp  # Ramer-Douglas-Peucker 算法
from scipy.spatial import ConvexHull
from Calculating_left_right_points import compute_boundaries
from TaoHang_turning import TaoHang_turning, TaoHang_turning_Transfer
from Wangfu_turning_operation import Wangfu_turning_operation
from calculate_angle_rad_begin_to_M import calculate_angle_rad_begin_to_M
from calculate_angles_between_lines import calculate_angles_between_lines
from calculate_line_angle import calculate_line_angle
from convert_local_path_to_latlon import convert_local_path_to_latlon
from coordinate_transform import latlon_to_local, local_to_latlon
from dubins import dubins_path_planning
from matplotlib.patches import Polygon as MplPolygon
from find_MN_Beisaier import find_MN
from find_intersection_point import find_intersection_point
from find_point_on_line import find_point_on_line
from fix_polygon_orientation import fix_shapely_polygon_orientation
from getting_tracking_poly_points import getting_tracking_poly_points
from insert_points_along_line import insert_points_along_line
from order_polygon_ccw import order_polygon_ccw
from scan_polygon_along_line_Beisaier import scan_polygon_with_intervals, scan_polygon_with_intervals_path
from spiral_path_generation_Beisaier import spiral_path_generation
from Beisaier_path_planning import Calculating_minimum_TaoHang_Nummber
from Beisaier_path_planning import Beisaier_path_planning_function

# region  地块和作业机具参数
# 拖拉机宽度/作业幅宽
#TRACTOR_WIDTH = 2.2 # 拖拉机宽度/作业幅宽 (米)
MIN_TURNING_RADIUS = 6.0  # 最小转弯半径 (米)  这里给大一点
curvature = 1/MIN_TURNING_RADIUS

# 拖拉机参数
TRACTOR_LENGTH = 2.8  # 车长
TRACTOR_WIDTH = 2.2  # 车宽
WHEEL_LEN = 0.8  # 车轮长度
WHEEL_WIDTH = 0.2  # 车轮宽度


# 起点
#start_point = Point(119.7352439, 32.3036982)
#start_point = Point(119.7515234, 32.3075920)
# 起点朝向角
#start_yaw_degree = 135.0 #单位为度

# 终点
#terminal_point = Point(119.7352439, 32.3036982)
#terminal_point = Point(119.7515234, 32.3075920)
# 终点朝向角
#terminal_yaw_degree = -45.0 #单位为度


# 2025年9月5日大全全覆盖实验
field_boundary_coords = [
     [119.7346358,  32.3008244],
     [119.7342016,  32.3006534],
     [119.7347161,  32.2998128],
     [119.7351016,  32.3000005]
]
# 起点
start_point = Point(119.7346358,  32.3008244)
# 起点朝向角
start_yaw_degree = -135.0 #单位为度
# 终点
terminal_point = Point(119.7346358,  32.3008244)
# 终点朝向角
terminal_yaw_degree = 45.0 #单位为度

'''
# 2025年9月9日校内实验01
field_boundary_coords = [
     [119.5020115,  32.1993409],
     [119.5021439,  32.1993252],
     [119.5021090,  32.1991402],
     [119.5019764,  32.1991527]
]
# 起点
start_point = Point(119.5019764,  32.1991527)
# 起点朝向角
start_yaw_degree = 25.0 #单位为度
# 终点
terminal_point = Point(119.5019764,  32.1991527)
# 终点朝向角
terminal_yaw_degree = -135.0 #单位为度
'''

'''
# 2025年9月9日校内实验02
field_boundary_coords = [
     [119.5023050,  32.1991732],
     [119.5024568,  32.1991542],
     [119.5024510,  32.1991006],
     [119.5022956,  32.1991240]
]
# 起点
start_point = Point(119.5023050,  32.1991732)
# 起点朝向角
start_yaw_degree = -25.0 #单位为度
# 终点
terminal_point = Point(119.5023050,  32.1991732)
# 终点朝向角
terminal_yaw_degree = 135.0 #单位为度
'''

# 原始边界坐标1
#field_boundary_coords = [
#    [119.7346032, 32.3038962],
#    [119.7352119, 32.3039580],
#    [119.7352439, 32.3036982],
#    [119.7347271, 32.3036666]
#]

# 原始边界坐标2
#field_boundary_coords = [
#    [119.7515234, 32.3075920],
#    [119.7500482, 32.3108438],
#    [119.7489564, 32.3105687],
#    [119.7506537, 32.3068446],
#    [119.7514286, 32.3068233]
#]

forward_interval = 0.20 #米
distance = 10 #米  10
# endregion

path_points = []  # 经纬度
path_yaws = []  # 角度



# region 画地图边界
# 重新排序坐标
ordered_field_boundary_coords = order_polygon_ccw(field_boundary_coords, start_point)
# 创建多边形对象
field_boundary_poly = Polygon(ordered_field_boundary_coords)
# 提取边界坐标
field_boundary_points = list(field_boundary_poly.exterior.coords)
# 分离经度和纬度
lons = [point[0] for point in field_boundary_points]
lats = [point[1] for point in field_boundary_points]
# 直接从多边形获取边界框
lon_min, lat_min, lon_max, lat_max = field_boundary_poly.bounds
# 设置起点为参考点
ref_lon = start_point.x
ref_lat = start_point.y
# 转换所有坐标点为局部坐标系坐标
local_coords = [latlon_to_local(lat, lon, ref_lat, ref_lon) for lon, lat in field_boundary_poly.exterior.coords]
# 创建局部坐标系下的多边形
local_field_boundary_poly = Polygon(local_coords)



local_field_boundary_Bians = []
for i in range(len(local_coords)-1):
    current_point = local_coords[i]
    next_point = local_coords[(i+1)%(len(local_coords)-1)]
    local_field_boundary_Bians.append([current_point, next_point])

local_field_angles_between_Bians = []
for i in range(len(local_field_boundary_Bians)-1):
    current_Bian = local_field_boundary_Bians[i]
    for j in range(i+1, len(local_field_boundary_Bians)):
        next_Bian = local_field_boundary_Bians[j]
        local_field_angles_between_Bians.append(calculate_angles_between_lines(current_Bian[0], current_Bian[1], next_Bian[0], next_Bian[1]))

TaoHang_Required_Numbers_List = []
TaoHang_Required_Widths_List = []
for i in range(len(local_field_angles_between_Bians)):
    start_yaw = local_field_angles_between_Bians[i]
    TaoHang_Required_Number, TaoHang_Required_Width = Calculating_minimum_TaoHang_Nummber(start_yaw, MIN_TURNING_RADIUS, TRACTOR_WIDTH)  # control_distance_limitation
    if TaoHang_Required_Number<100000000 and TaoHang_Required_Width<100000000:
        TaoHang_Required_Numbers_List.append(TaoHang_Required_Number)
        TaoHang_Required_Widths_List.append(TaoHang_Required_Width)


TaoHang_Required_max_Number = np.max(TaoHang_Required_Numbers_List)
TaoHang_Required_max_Width  = np.max(TaoHang_Required_Widths_List)
TaoHang_Required_max_Width_Number = int(np.ceil(TaoHang_Required_max_Width/TRACTOR_WIDTH))


# 创建地图和投影
fig, ax0 = plt.subplots(figsize=(16, 10))
#fig = plt.figure(figsize=(12, 10))
x_formatter = FormatStrFormatter('%1.5f')
y_formatter = FormatStrFormatter('%1.5f')
ax0.xaxis.set_major_formatter(x_formatter)
ax0.yaxis.set_major_formatter(y_formatter)
ax0.set_xlim(float(lon_min-0.00005), float(lon_max+0.00005))
ax0.set_ylim(float(lat_min-0.00005), float(lat_max+0.00005))
ax0.set_aspect('equal')
ax0.grid(True,linewidth=0.5, color='black', alpha=0.5, linestyle=':')
plt.rcParams['font.sans-serif']=['KaiTi']
plt.rcParams['axes.unicode_minus'] = False
ax0.set_title('雷公岛实验农场地块全覆盖路径规划图\nThe Completely Coverage Path Planning of Fields in Leigongju Island Experimental Farm', fontsize=20)
ax0.set_xlabel('经度坐标 Longitude Position', fontsize=15)
ax0.set_ylabel('纬度坐标 Latitude Position', fontsize=15)
# 添加图例和标题
#plt.rcParams['font.sans-serif']=['KaiTi']
#plt.rcParams['axes.unicode_minus'] = False
#plt.title('雷公岛实验农场地块全覆盖路径规划图\nThe Completely Coverage Path Planning of Fields in Leigongju Island Experimental Farm', fontsize=20)
#plt.tight_layout()


#ax = plt.axes(projection=ccrs.Mercator())
#ax.set_extent([lon_min-0.00005, lon_max+0.00005, lat_min-0.00005, lat_max+0.00005], crs=ccrs.PlateCarree())
# 设置网格
#gl = ax.gridlines(crs=ccrs.PlateCarree(), draw_labels=True, linewidth=0.5, color='black', alpha=0.5, linestyle=':')
#gl.xformatter = LONGITUDE_FORMATTER
#gl.yformatter = LATITUDE_FORMATTER
#gl.xlocator = plt.FixedLocator(np.arange(float(lon_min-0.00005), float(lon_max+0.00005), 0.00005))
#gl.ylocator = plt.FixedLocator(np.arange(float(lat_min-0.00005), float(lat_max+0.00005), 0.00005))
#gl.top_labels = False
#gl.right_labels = False
#gl.xlabel_style = {'size': 8}
#gl.ylabel_style = {'size': 8}
#画地块外边界点
#ax.scatter(lons, lats, transform=ccrs.PlateCarree(), color='red', edgecolor='red', s=20, zorder=3)
ax0.scatter(lons, lats, color='red', edgecolor='red', s=20, zorder=3)
#画起点
#ax.scatter(start_point.x, start_point.y, transform=ccrs.PlateCarree(), color='green', edgecolor='black', s=100, zorder=6)
ax0.scatter(start_point.x, start_point.y, color='green', edgecolor='black', s=100, zorder=6)
#画终点
#ax.scatter(terminal_point.x, terminal_point.y, transform=ccrs.PlateCarree(), color='red', edgecolor='black', s=100, zorder=6)
ax0.scatter(terminal_point.x, terminal_point.y, color='red', edgecolor='black', s=100, zorder=6)
#画边界线
#ax.plot(lons, lats, transform=ccrs.PlateCarree(), color='red', linewidth=2, linestyle='-')
ax0.plot(lons, lats, color='red', linewidth=2, linestyle='-')
# 闭合最后一条边
# ax.plot([lons[-1], lons[0]], [lats[-1], lats[0]], transform=ccrs.PlateCarree(), color='blue', linewidth=2, linestyle='-')
# endregion





# region 边界内缩和生成回字形作业参考边界
# 不断内缩TRACTOR_WIDTH,生成作业带边界
swath_boundaries = spiral_path_generation(local_field_boundary_poly, TRACTOR_WIDTH, MIN_TURNING_RADIUS, TaoHang_Required_max_Width_Number)
# 画出作业带边界
for local_boundary in swath_boundaries:
    # 转换局部坐标系坐标为经纬度坐标
    local_boundary_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_boundary.coords]
    # 分离经纬度坐标
    local_boundary_points_lons = [point[0] for point in local_boundary_points]
    local_boundary_points_lats = [point[1] for point in local_boundary_points]
    # 画边界点
    #ax.scatter(local_boundary_points_lons, local_boundary_points_lats, transform=ccrs.PlateCarree(), color='red', edgecolor='red', s=20, zorder=3)
    ax0.scatter(local_boundary_points_lons, local_boundary_points_lats, color='red', edgecolor='red', s=20, zorder=3)
    # 画转弯点
    # ax.scatter(local_boundary_points[0], local_boundary_points[1], transform=ccrs.PlateCarree(), color='red', edgecolor='black', s=100, zorder=6)
    # 画出螺旋线
    #ax.plot(local_boundary_points_lons, local_boundary_points_lats, transform=ccrs.PlateCarree(), color='red', linewidth=2, linestyle=':')
    ax0.plot(local_boundary_points_lons, local_boundary_points_lats, color='red', linewidth=2, linestyle=':')



#最内部的结束边界多边形
local_end_boundary_poly = Polygon(swath_boundaries[-1])
#已有的调头区宽度
nested_turning_area_width = TRACTOR_WIDTH*len(swath_boundaries)


local_Nested_operation_path_list = []
for local_boundary in swath_boundaries:
    local_boundary_poly = Polygon(local_boundary)
    # 作业带边界外扩TRACTOR_WIDTH/2,得到参考作业路径
    local_Nested_operation_path_ploy = local_boundary_poly.buffer(TRACTOR_WIDTH/2, join_style=2, mitre_limit=5.0)
    # 换成逆时针顺序
    local_Nested_operation_path_ploy = fix_shapely_polygon_orientation(local_Nested_operation_path_ploy)

    local_Nested_operation_path_list.append([[lon,lat] for lon,lat in local_Nested_operation_path_ploy.exterior.coords])




#local_inner_boundary_poly = local_field_boundary_poly.buffer(-TRACTOR_WIDTH/2, join_style=2, mitre_limit=5.0)
# 换成逆时针顺序
#local_inner_boundary_poly = fix_shapely_polygon_orientation(local_inner_boundary_poly)
#first_nested_operation_path = []
#first_nested_operation_path.append(local_inner_boundary_poly.exterior)
# 不断内缩TRACTOR_WIDTH,生成螺旋路径,即参考作业路径
#local_Nested_operation_path_list = spiral_path_generation(local_inner_boundary_poly, TRACTOR_WIDTH)
#local_Nested_operation_path_list = first_nested_operation_path+local_Nested_operation_path_list
for local_Nested_operation_path in local_Nested_operation_path_list:
    # 转换局部坐标系坐标为经纬度坐标
    Nested_operation_path_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_Nested_operation_path]

    Nested_operation_path_points_lons = [point[0] for point in Nested_operation_path_points]
    Nested_operation_path_points_lats = [point[1] for point in Nested_operation_path_points]
    # 画出参考作业路径
    #ax.plot(Nested_operation_path_points_lons, Nested_operation_path_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle=':')
    ax0.plot(Nested_operation_path_points_lons, Nested_operation_path_points_lats, color='black', linewidth=2, linestyle=':')
# endregion



# region 画回字形作业路径
local_start_x, local_start_y = latlon_to_local(start_point.y, start_point.x, ref_lat, ref_lon)
start_yaw = np.deg2rad(start_yaw_degree)  # [rad]

Beisaier_YuLiangLv = 1

for i in range(len(local_Nested_operation_path_list)):
    #第一条作业带
    if i==0:
        for j in range(len(local_Nested_operation_path_list[i])-1):
            if j==0:#第一条作业带第一个点
                # 第一段曲线
                first_x, first_y = local_Nested_operation_path_list[i][j]
                next_x, next_y = local_Nested_operation_path_list[i][j+1]
                next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]
                end_yaw = calculate_line_angle(first_x, first_y, next_x, next_y, False)  # [rad]
                end_x, end_y = find_point_on_line(first_x, first_y, next_x, next_y, distance)
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(local_start_x, local_start_y, start_yaw,
                                                                                       end_x, end_y, end_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第一段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='red', linewidth=2, linestyle='-.')
                ax0.plot(turning_lons, turning_lats, color='red', linewidth=2,
                        linestyle='-.')
                path_points = path_points+turning_points
                path_yaws = path_yaws+path_yaw_turning


                begin_x = end_x
                begin_y = end_y
                M,N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, MIN_TURNING_RADIUS*Beisaier_YuLiangLv+0.1, curvature, forward_interval)

                # 第二段直线
                local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                path_yaw_forward = [ ( calculate_angle_rad_begin_to_M(begin_x,begin_y,M[0],M[1]) ) ]*len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                forward_points_lons = [point[0] for point in forward_points]
                forward_points_lats = [point[1] for point in forward_points]
                # 画出第二段直线
                #ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                ax0.plot(forward_points_lons, forward_points_lats, color='black',                 linewidth=2, linestyle='-')
                path_points = path_points + forward_points
                path_yaws = path_yaws+path_yaw_forward


                # 第三段曲线
                M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(M[0], M[1], M_yaw,
                                                                                           N[0], N[1], N_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第三段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='purple', linewidth=2, linestyle='-')
                ax0.plot(turning_lons, turning_lats, color='purple', linewidth=2,
                        linestyle='-')
                path_points = path_points + turning_points
                path_yaws = path_yaws+path_yaw_turning



                begin_x = N[0]
                begin_y = N[1]

            elif not j==len(local_Nested_operation_path_list[i])-2:#第一条作业带，不是第一个点，也不是最后一个点
                current_x, current_y = local_Nested_operation_path_list[i][j]
                next_x, next_y = local_Nested_operation_path_list[i][j+1]
                next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]
                M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, MIN_TURNING_RADIUS*Beisaier_YuLiangLv + 0.1, curvature, forward_interval)

                # 第一段直线
                local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                path_yaw_forward = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                forward_points_lons = [point[0] for point in forward_points]
                forward_points_lats = [point[1] for point in forward_points]
                # 画出第一段直线
                #ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                ax0.plot(forward_points_lons, forward_points_lats, color='black',
                        linewidth=2, linestyle='-')
                path_points = path_points + forward_points
                path_yaws = path_yaws+path_yaw_forward


                # 第二段曲线
                M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(M[0], M[1], M_yaw,
                                                                                           N[0], N[1], N_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第二段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='purple', linewidth=2, linestyle='-')
                ax0.plot(turning_lons, turning_lats, color='purple', linewidth=2,    linestyle='-')
                path_points = path_points + turning_points
                path_yaws = path_yaws+path_yaw_turning


                begin_x = N[0]
                begin_y = N[1]
            else:#第一条作业带最后一个点
                current_x, current_y = local_Nested_operation_path_list[i][j]
                #next点为两个线段延伸后的交点
                next_x, next_y = find_intersection_point([begin_x,begin_y],local_Nested_operation_path_list[i][j + 1],local_Nested_operation_path_list[i+1][0],local_Nested_operation_path_list[i+1][1])
                #此时，下下个点为下一条作业带的第二个点
                next_next_x, next_next_y = local_Nested_operation_path_list[i+1][1]

                M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, MIN_TURNING_RADIUS*Beisaier_YuLiangLv + 0.1, curvature, forward_interval)

                # 第一段直线
                local_forward_points = insert_points_along_line(begin_x, begin_y, M[0], M[1], forward_interval)
                path_yaw_forward = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                forward_points_lons = [point[0] for point in forward_points]
                forward_points_lats = [point[1] for point in forward_points]
                # 画出第一段直线
                #ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                ax0.plot(forward_points_lons, forward_points_lats, color='black',   linewidth=2, linestyle='-')
                path_points = path_points + forward_points
                path_yaws = path_yaws+path_yaw_forward


                # 第二段曲线
                M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y, False)  # [rad]
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(M[0], M[1], M_yaw,
                                                                                           N[0], N[1], N_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第二段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='purple', linewidth=2, linestyle='-')
                ax0.plot(turning_lons, turning_lats, color='purple', linewidth=2,
                        linestyle='-')
                path_points = path_points + turning_points
                path_yaws = path_yaws+path_yaw_turning

                begin_x = N[0]
                begin_y = N[1]

    # 不是第一条作业带,也不是最后一条作业带
    elif not i==len(local_Nested_operation_path_list)-1:
        for j in range(len(local_Nested_operation_path_list[i])-1):
            if j==0:#是当前作业带第一个点
                first_x, first_y = local_Nested_operation_path_list[i][j]
                next_x, next_y = local_Nested_operation_path_list[i][j+1]
                next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]

                M,N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, MIN_TURNING_RADIUS*Beisaier_YuLiangLv+0.1, curvature, forward_interval)

                # 第一段直线
                local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                path_yaw_forward = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                forward_points_lons = [point[0] for point in forward_points]
                forward_points_lats = [point[1] for point in forward_points]
                # 画出第一段直线
                #ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                ax0.plot(forward_points_lons, forward_points_lats, color='black',            linewidth=2, linestyle='-')
                path_points = path_points + forward_points
                path_yaws = path_yaws+path_yaw_forward



                # 第二段曲线
                M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(M[0], M[1], M_yaw,
                                                                                           N[0], N[1], N_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第二段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='purple', linewidth=2, linestyle='-')
                ax0.plot(turning_lons, turning_lats, color='purple', linewidth=2,        linestyle='-')
                path_points = path_points + turning_points
                path_yaws = path_yaws+path_yaw_turning



                begin_x = N[0]
                begin_y = N[1]

            elif not j==len(local_Nested_operation_path_list[i])-2:#不是第一个点，也不是最后一个点
                current_x, current_y = local_Nested_operation_path_list[i][j]
                next_x, next_y = local_Nested_operation_path_list[i][j+1]
                next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]
                M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, MIN_TURNING_RADIUS*Beisaier_YuLiangLv + 0.1, curvature, forward_interval)

                # 第一段直线
                local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                path_yaw_forward = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                forward_points_lons = [point[0] for point in forward_points]
                forward_points_lats = [point[1] for point in forward_points]
                # 画出第一段直线
                #ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                ax0.plot(forward_points_lons, forward_points_lats, color='black',        linewidth=2, linestyle='-')
                path_points = path_points + forward_points
                path_yaws = path_yaws+path_yaw_forward



                # 第二段曲线
                M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(M[0], M[1], M_yaw,
                                                                                           N[0], N[1], N_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第二段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='purple', linewidth=2, linestyle='-')
                ax0.plot(turning_lons, turning_lats, color='purple', linewidth=2,    linestyle='-')
                path_points = path_points + turning_points
                path_yaws = path_yaws+path_yaw_turning



                begin_x = N[0]
                begin_y = N[1]
            else:#最后一个点
                current_x, current_y = local_Nested_operation_path_list[i][j]
                # next点为两个线段延伸后的交点
                next_x, next_y = find_intersection_point([begin_x, begin_y], local_Nested_operation_path_list[i][j + 1],
                                            local_Nested_operation_path_list[i + 1][0],
                                            local_Nested_operation_path_list[i + 1][1])
                #此时，下下个点为下一条作业带的第二个点
                next_next_x, next_next_y = local_Nested_operation_path_list[i+1][1]

                M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, MIN_TURNING_RADIUS*Beisaier_YuLiangLv + 0.1, curvature, forward_interval)

                # 第一段直线
                local_forward_points = insert_points_along_line(begin_x, begin_y, M[0], M[1], forward_interval)
                path_yaw_forward = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                forward_points_lons = [point[0] for point in forward_points]
                forward_points_lats = [point[1] for point in forward_points]
                # 画出第一段直线
                #ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                ax0.plot(forward_points_lons, forward_points_lats,  color='black',   linewidth=2, linestyle='-')
                path_points = path_points + forward_points
                path_yaws = path_yaws+path_yaw_forward



                # 第二段曲线
                M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y, False)  # [rad]
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(M[0], M[1], M_yaw,
                                                                                           N[0], N[1], N_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第二段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='purple', linewidth=2, linestyle='-')
                ax0.plot(turning_lons, turning_lats, color='purple', linewidth=2,       linestyle='-')
                path_points = path_points + turning_points
                path_yaws = path_yaws+path_yaw_turning



                begin_x = N[0]
                begin_y = N[1]

    # 是最后一条作业带
    else:
        for j in range(len(local_Nested_operation_path_list[i])-1):
            if j==0:#是当前作业带第一个点
                first_x, first_y = local_Nested_operation_path_list[i][j]
                next_x, next_y = local_Nested_operation_path_list[i][j+1]
                next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]

                M,N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, MIN_TURNING_RADIUS*Beisaier_YuLiangLv+0.1, curvature, forward_interval)

                # 第一段直线
                local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                path_yaw_forward = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                forward_points_lons = [point[0] for point in forward_points]
                forward_points_lats = [point[1] for point in forward_points]
                # 画出第一段直线
                #ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                ax0.plot(forward_points_lons, forward_points_lats,color='black',      linewidth=2, linestyle='-')
                path_points = path_points + forward_points
                path_yaws = path_yaws+path_yaw_forward




                # 第二段曲线
                M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(M[0], M[1], M_yaw,
                                                                                           N[0], N[1], N_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第二段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='purple', linewidth=2, linestyle='-')
                ax0.plot(turning_lons, turning_lats,color='purple', linewidth=2,     linestyle='-')
                path_points = path_points + turning_points
                path_yaws = path_yaws+path_yaw_turning



                begin_x = N[0]
                begin_y = N[1]

            elif not j==len(local_Nested_operation_path_list[i])-2:#不是第一个点，也不是最后一个点
                current_x, current_y = local_Nested_operation_path_list[i][j]
                next_x, next_y = local_Nested_operation_path_list[i][j+1]
                next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]
                M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, MIN_TURNING_RADIUS*Beisaier_YuLiangLv + 0.1, curvature, forward_interval)

                # 第一段直线
                if not (begin_x==M[0] and begin_y==M[1]):
                    local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                    path_yaw_forward = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                    forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                    # 分离经度和纬度
                    forward_points_lons = [point[0] for point in forward_points]
                    forward_points_lats = [point[1] for point in forward_points]
                    # 画出第一段直线
                    #ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                    ax0.plot(forward_points_lons, forward_points_lats,  color='black',  linewidth=2, linestyle='-')
                    path_points = path_points + forward_points
                    path_yaws = path_yaws+path_yaw_forward

                    M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                else:  # 如果已经没有直线段了  则直接从上一段曲线的最后一个点的朝向角，作为新曲线的初始点的朝向角
                    M_yaw = path_yaws[-1]



                # 第二段曲线
                #M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = Beisaier_path_planning_function(M[0], M[1], M_yaw,
                                                                                           N[0], N[1], N_yaw, curvature, forward_interval)
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                turning_lons = [point[0] for point in turning_points]
                turning_lats = [point[1] for point in turning_points]
                # 画出第二段曲线
                #ax.plot(turning_lons, turning_lats, transform=ccrs.PlateCarree(), color='purple', linewidth=2, linestyle='-')
                ax0.plot(turning_lons, turning_lats, color='purple', linewidth=2,  linestyle='-')
                path_points = path_points + turning_points
                path_yaws = path_yaws+path_yaw_turning



                begin_x = N[0]
                begin_y = N[1]
            else:#最后一个点
                first_point = [begin_x, begin_y]
                second_point = local_Nested_operation_path_list[i][j+1]
                # 将second_point向内部移动TRACTOR_WIDTH/2
                second_point_x, second_point_y = find_point_on_line(second_point[0], second_point[1], first_point[0], first_point[1], TRACTOR_WIDTH/2)
                second_point = [second_point_x, second_point_y]

                # 直线
                local_forward_points = insert_points_along_line(first_point[0], first_point[1], second_point[0],
                                                                second_point[1], forward_interval)
                path_yaw_forward = [(calculate_angle_rad_begin_to_M(first_point[0], first_point[1], second_point[0],
                                                            second_point[0]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                forward_points_lons = [point[0] for point in forward_points]
                forward_points_lats = [point[1] for point in forward_points]
                # 画出之前的那段直线
                # ax.plot(forward_points_lons, forward_points_lats, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle='-')
                ax0.plot(forward_points_lons, forward_points_lats, color='black', linewidth=2, linestyle='-')
                path_points = path_points + forward_points
                path_yaws = path_yaws + path_yaw_forward

# endregion

Before_TaoHang_transfer_start_point = second_point
Before_TaoHang_transfer_start_yaw = path_yaws[-1]

"""

"""
#control_distance_limitation = TRACTOR_WIDTH*len(local_Nested_operation_path_list)
#TaoHang_Required_Numbers, TaoHang_Required_Width = Calculating_minimum_TaoHang_Nummber(MIN_TURNING_RADIUS, TRACTOR_WIDTH, control_distance_limitation)

# region 画往复路径

#已得到first_point和second_point
local_intersection_points_list = scan_polygon_with_intervals(local_end_boundary_poly, first_point, second_point, TRACTOR_WIDTH)
cleaned_local_intersection_points_list = [item for item in local_intersection_points_list if item]
for i in range(len(cleaned_local_intersection_points_list)):
    intersection_points_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in cleaned_local_intersection_points_list[i]]
    intersection_points_points_lons = [point[0] for point in intersection_points_points]
    intersection_points_points_lats = [point[1] for point in intersection_points_points]
    # 画边界点
    #ax.scatter(intersection_points_points_lons, intersection_points_points_lats, transform=ccrs.PlateCarree(), color='red',    edgecolor='red', s=20, zorder=3)
    ax0.scatter(intersection_points_points_lons, intersection_points_points_lats, color='red', edgecolor='red', s=20, zorder=3)
    # 画出参考作业路径界限
    #ax.plot(intersection_points_points_lons, intersection_points_points_lats, transform=ccrs.PlateCarree(), color='red', linewidth=2, linestyle=':')
    ax0.plot(intersection_points_points_lons, intersection_points_points_lats, color='red', linewidth=2, linestyle=':')





#已得到first_point和second_point
cleaned_local_intersection_points_list_path = scan_polygon_with_intervals_path(local_end_boundary_poly, first_point, second_point, TRACTOR_WIDTH, TaoHang_Required_max_Number)
#local_intersection_points_list_path
#cleaned_local_intersection_points_list_path = [item for item in local_intersection_points_list_path if item]
for i in range(len(cleaned_local_intersection_points_list_path)):
    intersection_points_points_path = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in cleaned_local_intersection_points_list_path[i]]
    intersection_points_points_lons_path = [point[0] for point in intersection_points_points_path]
    intersection_points_points_lats_path = [point[1] for point in intersection_points_points_path]
    # 画出参考作业路径
    #ax.plot(intersection_points_points_lons_path, intersection_points_points_lats_path, transform=ccrs.PlateCarree(), color='black', linewidth=2, linestyle=':')
    ax0.plot(intersection_points_points_lons_path, intersection_points_points_lats_path, color='black', linewidth=2, linestyle=':')





# 从开始点到套行开始点的那段调头转移曲线
next_Hang_index = TaoHang_Required_max_Number
if np.sqrt((cleaned_local_intersection_points_list_path[next_Hang_index][0][0]-Before_TaoHang_transfer_start_point[0])**2+
           (cleaned_local_intersection_points_list_path[next_Hang_index][0][1]-Before_TaoHang_transfer_start_point[1])**2) <= \
    np.sqrt((cleaned_local_intersection_points_list_path[next_Hang_index][1][0]-Before_TaoHang_transfer_start_point[0])**2+
             (cleaned_local_intersection_points_list_path[next_Hang_index][1][1]-Before_TaoHang_transfer_start_point[1])**2):
    Next_TaoHang_start_point = cleaned_local_intersection_points_list_path[next_Hang_index][0]
    Next_TaoHang_end_point = cleaned_local_intersection_points_list_path[next_Hang_index][1]
    Next_TaoHang_start_yaw = calculate_line_angle(Next_TaoHang_start_point[0], Next_TaoHang_start_point[1], Next_TaoHang_end_point[0], Next_TaoHang_end_point[1], degrees=False)
    Next_TaoHang_end_yaw = Next_TaoHang_start_yaw
else:
    Next_TaoHang_start_point = cleaned_local_intersection_points_list_path[next_Hang_index][1]
    Next_TaoHang_end_point = cleaned_local_intersection_points_list_path[next_Hang_index][0]
    Next_TaoHang_start_yaw = calculate_line_angle(Next_TaoHang_start_point[0], Next_TaoHang_start_point[1], Next_TaoHang_end_point[0], Next_TaoHang_end_point[1], degrees=False)
    Next_TaoHang_end_yaw = Next_TaoHang_start_yaw


new_found_start_point, Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = TaoHang_turning(Before_TaoHang_transfer_start_point, Before_TaoHang_transfer_start_yaw,
                                                                               Next_TaoHang_start_point, Next_TaoHang_start_yaw, curvature, TRACTOR_WIDTH, forward_interval, local_field_boundary_poly)
turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
turning_lons = [point[0] for point in turning_points]
turning_lats = [point[1] for point in turning_points]
# 画出调头段曲线
ax0.plot(turning_lons, turning_lats, color='blue', linewidth=2, linestyle='-')
path_points = path_points + turning_points
path_yaws = path_yaws + path_yaw_turning


#TaoHang_Required_max_Number
#TaoHang_Required_max_Width
#TaoHang_Required_max_Width_Number

for i in range(TaoHang_Required_max_Number, len(cleaned_local_intersection_points_list_path)):
    if not i==len(cleaned_local_intersection_points_list_path)-1:
        Current_TaoHang_start_point = Next_TaoHang_start_point
        Current_TaoHang_end_point = Next_TaoHang_end_point
        Current_TaoHang_start_yaw = Next_TaoHang_start_yaw
        Current_TaoHang_end_yaw = Next_TaoHang_end_yaw

        # 先算出到下一返回行的套行调头曲线的可行的起始点
        next_TaoHang_index = i - TaoHang_Required_max_Number
        if np.sqrt((cleaned_local_intersection_points_list_path[next_TaoHang_index][0][0] - Current_TaoHang_end_point[
            0]) ** 2 +
                   (cleaned_local_intersection_points_list_path[next_TaoHang_index][0][1] - Current_TaoHang_end_point[
                       1]) ** 2) <= \
                np.sqrt((cleaned_local_intersection_points_list_path[next_TaoHang_index][1][0] -
                         Current_TaoHang_end_point[0]) ** 2 +
                        (cleaned_local_intersection_points_list_path[next_TaoHang_index][1][1] -
                         Current_TaoHang_end_point[1]) ** 2):
            Next_TaoHang_start_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][0]
            Next_TaoHang_end_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][1]
            Next_TaoHang_start_yaw = calculate_line_angle(Next_TaoHang_start_point[0], Next_TaoHang_start_point[1],
                                                          Next_TaoHang_end_point[0], Next_TaoHang_end_point[1],
                                                          degrees=False)
            Next_TaoHang_end_yaw = Next_TaoHang_start_yaw
        else:
            Next_TaoHang_start_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][1]
            Next_TaoHang_end_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][0]
            Next_TaoHang_start_yaw = calculate_line_angle(Next_TaoHang_start_point[0], Next_TaoHang_start_point[1],
                                                          Next_TaoHang_end_point[0], Next_TaoHang_end_point[1],
                                                          degrees=False)
            Next_TaoHang_end_yaw = Next_TaoHang_start_yaw

        new_found_start_point, Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = TaoHang_turning(Current_TaoHang_end_point,
                                                                                       Current_TaoHang_end_yaw,
                                                                                       Next_TaoHang_start_point,
                                                                                       Next_TaoHang_start_yaw,
                                                                                       curvature, TRACTOR_WIDTH,
                                                                                       forward_interval,
                                                                                       local_field_boundary_poly)
        # 算出new_found_start_point后，先画出当前行的直线部分
        local_forward_points = insert_points_along_line(Current_TaoHang_start_point[0], Current_TaoHang_start_point[1],
                                                        new_found_start_point[0], new_found_start_point[1],
                                                        forward_interval)
        path_yaw_forward = [(calculate_angle_rad_begin_to_M(Current_TaoHang_start_point[0], Current_TaoHang_start_point[1],
                                                    new_found_start_point[0], new_found_start_point[1]))] * len(
            local_forward_points)
        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
        # 分离经度和纬度
        forward_points_lons = [point[0] for point in forward_points]
        forward_points_lats = [point[1] for point in forward_points]
        ax0.plot(forward_points_lons, forward_points_lats, color='black', linewidth=2, linestyle='-')
        path_points = path_points + forward_points
        path_yaws = path_yaws + path_yaw_forward
        # 再画出到下一返回行的套行调头曲线
        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
        turning_lons = [point[0] for point in turning_points]
        turning_lats = [point[1] for point in turning_points]
        ax0.plot(turning_lons, turning_lats, color='blue', linewidth=2, linestyle='-')
        path_points = path_points + turning_points
        path_yaws = path_yaws + path_yaw_turning


        # 再算出下一返回行到下下返回行的套行调头曲线的可行的起始点
        Current_TaoHang_start_point = Next_TaoHang_start_point
        Current_TaoHang_end_point = Next_TaoHang_end_point
        Current_TaoHang_start_yaw = Next_TaoHang_start_yaw
        Current_TaoHang_end_yaw = Next_TaoHang_end_yaw

        next_TaoHang_index = i + 1
        if np.sqrt((cleaned_local_intersection_points_list_path[next_TaoHang_index][0][0] - Current_TaoHang_end_point[
            0]) ** 2 +
                   (cleaned_local_intersection_points_list_path[next_TaoHang_index][0][1] - Current_TaoHang_end_point[
                       1]) ** 2) <= \
                np.sqrt((cleaned_local_intersection_points_list_path[next_TaoHang_index][1][0] -
                         Current_TaoHang_end_point[0]) ** 2 +
                        (cleaned_local_intersection_points_list_path[next_TaoHang_index][1][1] -
                         Current_TaoHang_end_point[1]) ** 2):
            Next_TaoHang_start_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][0]
            Next_TaoHang_end_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][1]
            Next_TaoHang_start_yaw = calculate_line_angle(Next_TaoHang_start_point[0], Next_TaoHang_start_point[1],
                                                          Next_TaoHang_end_point[0], Next_TaoHang_end_point[1],
                                                          degrees=False)
            Next_TaoHang_end_yaw = Next_TaoHang_start_yaw
        else:
            Next_TaoHang_start_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][1]
            Next_TaoHang_end_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][0]
            Next_TaoHang_start_yaw = calculate_line_angle(Next_TaoHang_start_point[0], Next_TaoHang_start_point[1],
                                                          Next_TaoHang_end_point[0], Next_TaoHang_end_point[1],
                                                          degrees=False)
            Next_TaoHang_end_yaw = Next_TaoHang_start_yaw

        new_found_start_point, Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = TaoHang_turning(Current_TaoHang_end_point,
                                                                                       Current_TaoHang_end_yaw,
                                                                                       Next_TaoHang_start_point,
                                                                                       Next_TaoHang_start_yaw,
                                                                                       curvature, TRACTOR_WIDTH,
                                                                                       forward_interval,
                                                                                       local_field_boundary_poly)
        # 算出new_found_start_point后，先画出当前行的直线部分
        local_forward_points = insert_points_along_line(Current_TaoHang_start_point[0], Current_TaoHang_start_point[1],
                                                        new_found_start_point[0], new_found_start_point[1],
                                                        forward_interval)
        path_yaw_forward = [(calculate_angle_rad_begin_to_M(Current_TaoHang_start_point[0], Current_TaoHang_start_point[1],
                                                    new_found_start_point[0], new_found_start_point[1]))] * len(
            local_forward_points)
        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
        # 分离经度和纬度
        forward_points_lons = [point[0] for point in forward_points]
        forward_points_lats = [point[1] for point in forward_points]
        ax0.plot(forward_points_lons, forward_points_lats, color='black', linewidth=2, linestyle='-')
        path_points = path_points + forward_points
        path_yaws = path_yaws + path_yaw_forward
        # 再画出到下一返回行的套行调头曲线
        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
        turning_lons = [point[0] for point in turning_points]
        turning_lats = [point[1] for point in turning_points]
        ax0.plot(turning_lons, turning_lats, color='blue', linewidth=2, linestyle='-')
        path_points = path_points + turning_points
        path_yaws = path_yaws + path_yaw_turning

    else:
        Current_TaoHang_start_point = Next_TaoHang_start_point
        Current_TaoHang_end_point = Next_TaoHang_end_point
        Current_TaoHang_start_yaw = Next_TaoHang_start_yaw
        Current_TaoHang_end_yaw = Next_TaoHang_end_yaw

        next_TaoHang_index = i - TaoHang_Required_max_Number
        if np.sqrt((cleaned_local_intersection_points_list_path[next_TaoHang_index][0][0] - Current_TaoHang_end_point[
            0]) ** 2 +
                   (cleaned_local_intersection_points_list_path[next_TaoHang_index][0][1] - Current_TaoHang_end_point[
                       1]) ** 2) <= \
                np.sqrt((cleaned_local_intersection_points_list_path[next_TaoHang_index][1][0] -
                         Current_TaoHang_end_point[0]) ** 2 +
                        (cleaned_local_intersection_points_list_path[next_TaoHang_index][1][1] -
                         Current_TaoHang_end_point[1]) ** 2):
            Next_TaoHang_start_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][0]
            Next_TaoHang_end_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][1]
            Next_TaoHang_start_yaw = calculate_line_angle(Next_TaoHang_start_point[0], Next_TaoHang_start_point[1],
                                                          Next_TaoHang_end_point[0], Next_TaoHang_end_point[1],
                                                          degrees=False)
            Next_TaoHang_end_yaw = Next_TaoHang_start_yaw
        else:
            Next_TaoHang_start_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][1]
            Next_TaoHang_end_point = cleaned_local_intersection_points_list_path[next_TaoHang_index][0]
            Next_TaoHang_start_yaw = calculate_line_angle(Next_TaoHang_start_point[0], Next_TaoHang_start_point[1],
                                                          Next_TaoHang_end_point[0], Next_TaoHang_end_point[1],
                                                          degrees=False)
            Next_TaoHang_end_yaw = Next_TaoHang_start_yaw

        new_found_start_point, Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = TaoHang_turning(Current_TaoHang_end_point,
                                                                                       Current_TaoHang_end_yaw,
                                                                                       Next_TaoHang_start_point,
                                                                                       Next_TaoHang_start_yaw,
                                                                                       curvature, TRACTOR_WIDTH,
                                                                                       forward_interval,
                                                                                       local_field_boundary_poly)
        # 直线部分
        local_forward_points = insert_points_along_line(Current_TaoHang_start_point[0], Current_TaoHang_start_point[1],
                                                        new_found_start_point[0], new_found_start_point[1],
                                                        forward_interval)
        path_yaw_forward = [(calculate_angle_rad_begin_to_M(Current_TaoHang_start_point[0], Current_TaoHang_start_point[1],
                                                    new_found_start_point[0], new_found_start_point[1]))] * len(
            local_forward_points)
        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
        # 分离经度和纬度
        forward_points_lons = [point[0] for point in forward_points]
        forward_points_lats = [point[1] for point in forward_points]
        ax0.plot(forward_points_lons, forward_points_lats, color='black', linewidth=2, linestyle='-')
        path_points = path_points + forward_points
        path_yaws = path_yaws + path_yaw_forward

        # 调头曲线部分
        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
        turning_lons = [point[0] for point in turning_points]
        turning_lats = [point[1] for point in turning_points]
        ax0.plot(turning_lons, turning_lats, color='blue', linewidth=2, linestyle='-')
        path_points = path_points + turning_points
        path_yaws = path_yaws + path_yaw_turning



        # 下一返回行
        Current_TaoHang_start_point = Next_TaoHang_start_point
        Current_TaoHang_end_point = Next_TaoHang_end_point
        Current_TaoHang_start_yaw = Next_TaoHang_start_yaw
        Current_TaoHang_end_yaw = Next_TaoHang_end_yaw

        local_terminal_point_x, local_terminal_point_y = latlon_to_local(terminal_point.y, terminal_point.x, ref_lat, ref_lon)
        terminal_yaw = np.deg2rad(terminal_yaw_degree)  # [rad]
        # new_found_start_point为新找到的起点
        new_found_start_point, Planed_OR_not, local_path_x, local_path_y, path_yaw_turning, lengths = TaoHang_turning_Transfer(Current_TaoHang_end_point, Current_TaoHang_end_yaw,
                                                                                       [local_terminal_point_x, local_terminal_point_y], terminal_yaw,
                                                                                       curvature, TRACTOR_WIDTH, forward_interval, local_field_boundary_poly)

        # 再画出下一返回行的直线部分
        local_forward_points = insert_points_along_line(Current_TaoHang_start_point[0], Current_TaoHang_start_point[1],
                                                        new_found_start_point[0], new_found_start_point[1], forward_interval)
        path_yaw_forward = [(calculate_angle_rad_begin_to_M(Current_TaoHang_start_point[0], Current_TaoHang_start_point[1],
                                                    new_found_start_point[0], new_found_start_point[1]))] * len(local_forward_points)
        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
        # 分离经度和纬度
        forward_points_lons = [point[0] for point in forward_points]
        forward_points_lats = [point[1] for point in forward_points]
        ax0.plot(forward_points_lons, forward_points_lats, color='black', linewidth=2, linestyle='-')
        path_points = path_points + forward_points
        path_yaws = path_yaws + path_yaw_forward

        # 再画出下一返回行 返回总起点的调头曲线部分
        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
        turning_lons = [point[0] for point in turning_points]
        turning_lats = [point[1] for point in turning_points]
        # 画出调头段曲线
        ax0.plot(turning_lons, turning_lats, color='blue', linewidth=2, linestyle='-')
        path_points = path_points + turning_points
        path_yaws = path_yaws + path_yaw_turning
# endregion



# 添加图例和标题
plt.rcParams['font.sans-serif']=['KaiTi']
plt.rcParams['axes.unicode_minus'] = False
plt.title('雷公岛实验农场地块全覆盖路径规划图\nThe Completely Coverage Path Planning of Fields in Leigongju Island Experimental Farm', fontsize=20)
plt.tight_layout()



# 把路径点保存成txt文件
np.savetxt("./Planning_result/map.txt", path_points, fmt='%.14f, %.15f')

path = np.column_stack((path_points, path_yaws))
np.savetxt("./Planning_result/path.txt", path, fmt='%.14f, %.15f, %.5f')


left_boundary_points, right_boundary_points = compute_boundaries(path, ref_lat, ref_lon, TRACTOR_WIDTH, ax0)
# 检查左右点数组长度是否一致
if len(left_boundary_points) != len(right_boundary_points):
    raise ValueError("左边界点数组和右边界点数组长度不一致")
else:
    # 构建作业带多边形列表（由四边形组成）
    n = len(left_boundary_points)
    polygons_list = []
    for i in range(n-1):
        # 获取当前点和下一个点的左右边界点
        left_i = left_boundary_points[i]
        left_i1 = left_boundary_points[i + 1]
        right_i = right_boundary_points[i]
        right_i1 = right_boundary_points[i + 1]
        # 构建四边形多边形（点顺序为顺时针或逆时针一致）
        poly = Polygon([left_i, left_i1, right_i1, right_i])
        polygons_list.append(poly)
        x, y = poly.exterior.xy
        patch = MplPolygon(list(zip(x, y)), closed=False, fill=True, edgecolor='green', facecolor='green', alpha=1.0)
        ax0.add_patch(patch)


plt.show()






"""
def create_car(lon, lat, theta):
    # 创建小车图形组件
    CAR_LENGTH = TRACTOR_LENGTH
    CAR_WIDTH = TRACTOR_WIDTH
    x, y = latlon_to_local(lat, lon, ref_lat, ref_lon)
    # 原点坐标转成经纬度下的原点坐标
    origin_lon, origin_lat = local_to_latlon(x-CAR_LENGTH/2, y-CAR_WIDTH/2, ref_lat, ref_lon)
    temp_lon, temp_lat = local_to_latlon(x+CAR_LENGTH/2, y+CAR_WIDTH/2, ref_lat, ref_lon)
    latlon_CAR_LENGTH = temp_lon - origin_lon
    latlon_CAR_WIDTH = temp_lat - origin_lat
    # 车身主体（矩形）经纬度坐标系下
    latlon_car_body = Rectangle((origin_lon, origin_lat),
                         latlon_CAR_LENGTH, latlon_CAR_WIDTH,
                         angle=np.degrees(theta), rotation_point='center',
                         color='Orange', alpha=1.0)
    return latlon_car_body, latlon_CAR_LENGTH , latlon_CAR_WIDTH
"""




"""
# 动画更新函数
def update(frame):
    # 更新每一帧的小车位置和方向
    CAR_LENGTH = TRACTOR_LENGTH
    CAR_WIDTH = TRACTOR_WIDTH
    lon, lat, theta = path[frame]
    if frame+1==len(path):
        plt.close(fig)
    # 将弧度转换为角度
    heading_deg = np.degrees(theta)
    info_text = f"当前全覆盖作业进度(Current Coverage Progress): {frame + 1}/{len(path)}\n当前农机位置(Current Position): ({lon:.6f}°, {lat:.6f}°)\n当前农机朝向角(Current Heading): {theta:.2f} rad ({heading_deg:.2f}°)"
    info_box.set_text(info_text)
    # ================================
    x, y = latlon_to_local(lat, lon, ref_lat, ref_lon)
    # 原点坐标转成经纬度下的原点坐标
    origin_lon, origin_lat = local_to_latlon(x-CAR_LENGTH/2, y-CAR_WIDTH/2, ref_lat, ref_lon)
    # 更新车身位置和方向
    car_body.set_xy([origin_lon, origin_lat])
    car_body.angle = np.degrees(theta)
    # 更新方向指示器
    arrow_len = CAR_LENGTH * 1.2
    end_x = x + arrow_len * np.cos(theta)
    end_y = y + arrow_len * np.sin(theta)
    latlon_end_x, latlon_end_y = local_to_latlon(end_x,end_y, ref_lat, ref_lon)
    direction_line.set_data([lon, latlon_end_x], [lat, latlon_end_y])
    direction_arrow.xy = (latlon_end_x, latlon_end_y)
    direction_arrow.set_position((lon, lat))
    covered_rect = Rectangle((origin_lon, origin_lat),
                         latlon_CAR_LENGTH, latlon_CAR_WIDTH,
                         angle=np.degrees(theta), rotation_point='center',
                         color='Orange', alpha=0.1)
    ax0.add_patch(covered_rect)
    covered_rects.append(covered_rect)
    return [car_body] + [direction_line, direction_arrow] + covered_rects + [info_box]
"""




"""
# 创建小车初始位置
car_body, latlon_CAR_LENGTH, latlon_CAR_WIDTH = create_car(path[0, 0], path[0, 1], path[0, 2])
ax0.add_patch(car_body)
# 添加方向指示器
direction_line, = ax0.plot([], [], 'r-', linewidth=2)
direction_arrow = ax0.annotate('', xy=(0,0), xytext=(0,0), arrowprops=dict(arrowstyle='->', color='red'))
# 存储覆盖区域的所有矩形
covered_rects = []
# ====== 新增部分：创建信息文本框 ======
# 使用固定位置（图形坐标系，0-1之间）
info_box = ax0.text(0.02, 0.03, '',
                   transform=ax0.transAxes,  # 使用图形坐标系
                   bbox=dict(boxstyle='round', facecolor='white', alpha=0.8),
                   fontsize=10, fontweight='bold')

# 创建动画
ani = FuncAnimation(fig, update, frames=len(path), interval=10, blit=True)

plt.show()
"""